#include "DataSaver.h"

#include "PointCloudHandler.h"
#include "ImageHandler.h"
#include "Utils.h"

using namespace Poco;

#pragma region Init
DataSaver::DataSaver(std::string config_file, Poco::Int32 id)
    : _id(id)
{
    Path configPath(config_file);
    configPath.setFileName("config.ini");
    Util::IniFileConfiguration::Ptr pConfig = new Util::IniFileConfiguration(configPath.toString());

    std::string topic = "lidar" + std::to_string(_id);
    uint16_t port = pConfig->getUInt(topic + ".port", 8080);
    std::string localip = pConfig->getString("network.localip", "");
    std::string remoteip = pConfig->getString("network.remoteip", "");
    std::string correctionfile = pConfig->getString(topic + ".file", "");

    // lidar
    std::vector<LidarConfigurationOptions *> _options;
    LidarConfigurationOptions *_option1 = new LidarConfigurationOptions();
    _option1->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
    _option1->SetValue(LidarConfigurationOptions::Data_Callback_DecType, LidarDecodeType::RAW);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_RemoteIP, remoteip);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_LocalIP, localip);
    _option1->SetValue(LidarConfigurationOptions::Data_Decode_CorrectionFile, correctionfile);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)port);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::AT_128);
    _options.push_back(_option1);

    std::cout << "Start create Datamanager " << _id << std::endl;
    _lidarDataManager = LidarDataManager::getInstance();
    _lidarDataManager->UseOptions(_options);
    _lidarDataManager->addPointCloudMsgCallback(std::bind(&DataSaver::processData, this, std::placeholders::_1));
    std::cout << "Finish create Datamanager " << _id << std::endl;

    // save
    _save_file_path = pConfig->getString(topic + ".savefile", "");
}

DataSaver::~DataSaver()
{
    stop();
    delete _lidarDataManager;
    _lidarDataManager = nullptr;
    delete _encoder;
    _encoder = nullptr;
}

void DataSaver::start()
{
    _lidarDataManager->start();

    _is_saving = true;
    _threadSaveAsStream.setName("threadSaveAsStream");
    _threadSaveAsStream.start(&DataSaver::threadSaveAsStream, this);
}

void DataSaver::stop()
{
    _is_saving = false;
    _threadSaveAsStream.tryJoin(5000);
    _lidarDataManager->stop();
}
#pragma endregion

#pragma region Save
void DataSaver::threadSaveAsStream(void *p)
{
    DataSaver *pThis = static_cast<DataSaver *>(p);
    pThis->saveAsStream();
}

void DataSaver::saveAsStream()
{
    std::cout << "Start saveAsStream " << _id << std::endl;

    while (_is_saving)
    {
        auto startTime = std::chrono::system_clock::now();

        if (_encoder == nullptr)
        {
            pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
            pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;
            _encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(compressionProfile, true, 0.001, 0.01, false, 20, false, 8);
        }

        if (cloud_queue.size() > 0)
        {
            // 提取出点云
            PointCloudData<pcl::PointXYZI> pointcloud;
            {
                std::lock_guard<std::mutex> lock(_mtx_saver);
                const PointCloudData<pcl::PointXYZI> &front_data = cloud_queue.front();
                pcl::PointCloud<pcl::PointXYZI>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZI>);
                pcl::copyPointCloud(*front_data.cloud_ptr, *new_cloud);

                pointcloud.timestamp = front_data.timestamp;
                pointcloud.frame_id = front_data.frame_id;
                pointcloud.lidar_id = front_data.lidar_id;
                pointcloud.sequence = front_data.sequence;
                pointcloud.cloud_ptr = new_cloud;
                // cloud_queue.pop();
            }

            // 1fps编码
            double tempStamp = pointcloud.timestamp;
            if (tempStamp - lastTimeStamp >= 0)
            {
                // 记录当前帧位置
                std::streampos frame_pos = _stream.tellp();

                lastTimeStamp = tempStamp;
                encodeNum++;
                pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud(new pcl::PointCloud<pcl::PointXYZ>());
                pcl::copyPointCloud(*pointcloud.cloud_ptr, *_cloud); // 1ms

                //_stream.write(reinterpret_cast<const char *>(&net_timestamp), sizeof(uint64_t));
                _encoder->encodePointCloud(_cloud->makeShared(), _stream);

                uint64_t host_timestamp = static_cast<uint64_t>(tempStamp * 1000);
                uint64_t net_timestamp = htonll(host_timestamp);
                stream_index.emplace_back(host_timestamp, frame_pos);

                if (startTimeStamp == 0)
                {
                    startTimeStamp = pointcloud.timestamp; // 记录文件起始时间
                }
            }

            if (tempStamp - startTimeStamp >= 1 * 10 * 60) // 1*60s一记录
            {
                std::string file_name = Utils::TimeStamp2DateTimeString(startTimeStamp) + "-" + Utils::TimeStamp2DateTimeString(tempStamp) + ".stream";
                std::ofstream output(_save_file_path + file_name, std::ios::binary);

                // 临时文件头
                char header[16] = {0};
                output.write(header, 16);

                // 数据区
                output << _stream.rdbuf();

                // 索引区
                const std::streampos index_start = output.tellp();
                // 索引条数目
                uint64_t entry_count = htonll(stream_index.size());
                output.write(reinterpret_cast<const char *>(&entry_count), sizeof(uint64_t));
                // 索引条目
                for (const auto &[ts, pos] : stream_index)
                {
                    uint64_t net_ts = htonll(ts);
                    uint64_t net_pos = htonll(static_cast<uint64_t>(pos)); // 在数据块中的偏移量
                    output.write(reinterpret_cast<const char *>(&net_ts), sizeof(uint64_t));
                    output.write(reinterpret_cast<const char *>(&net_pos), sizeof(uint64_t));
                }

                // 回填文件头
                output.seekp(0);
                uint64_t net_index_start = htonll(static_cast<uint64_t>(index_start));
                output.write(reinterpret_cast<const char *>(&net_index_start), sizeof(uint64_t));
                output.write(reinterpret_cast<const char *>(&entry_count), sizeof(uint64_t));

                output.close();
                _stream.str("");

                std::cout << file_name << " , " << encodeNum << std::endl;

                encodeNum = 0;
                startTimeStamp = 0;
                lastTimeStamp = 0;
                delete _encoder;
                _encoder = nullptr;
                stream_index.clear();
            }
        }

        auto endTime = std::chrono::system_clock::now();
        int sleepTimeSpan = MAX(1000 - std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime).count(), 0);
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepTimeSpan));
    }
}
#pragma endregion

#pragma region data
bool DataSaver::processData(PointCloudData<pcl::PointXYZI> &data)
{
    auto size = data.cloud_ptr->size();
    if (size == 0)
        return false;
    {
        std::lock_guard<std::mutex> lock(_mtx_saver);
        cloud_queue.push(data);
        if (cloud_queue.size() > 1)
            cloud_queue.pop();
    }
    return true;
}
#pragma endregion